module build_lattice
use pointer_lattice
use madx_ptc_module !, only: m_u

implicit none
  public
real(dp) :: eps_gino=1.d-7
contains





subroutine compute_lattice_a_for_node(pos2,ai)
implicit none
INTEGER ND2,NPARA
TYPE(DAMAP) ID
TYPE(REAL_8) Y(6)
TYPE(NORMALFORM) N
INTEGER I, order,k,pos1,pos2
TYPE(FIBRE), POINTER :: P
INTEGER, ALLOCATABLE :: J(:)
REAL(DP) fix0(6)

real(dp),  intent(inout) :: ai(6,6)
logical u



order=1

call move_to(my_ering,p,MY_START)

write(6,*) my_fix

!write(6,*) " Find Fixed Point "
!read(5,*) fix_p
!MY_FIX=0.d0
my_fix(5)=my_delta
 CALL FIND_ORBIT(my_ering,MY_FIX,MY_START,my_estate,eps_gino)

write(6,*) my_fix
if(p%t1%pos/=pos2) then
! call TRACK_beam_x( my_ering,MY_FIX,U,my_estate,POS1=p%t1%pos,POS2=pos2 )
call TRACK_probe_x(my_ering,MY_FIX,my_estate,node1=p%t1%pos,node2=pos2 )
u=.not.check_stable
 write(6,*)p%t1%pos,pos2,p%t2%pos
endif




CALL INIT(my_estate,order,MY_NP,BERZ,ND2,NPARA)

CALL ALLOC(ID);CALL ALLOC(Y);CALL ALLOC(N);
id=1
Y=NPARA

Y=MY_FIX+ID

! call TRACK_beam_x( my_ering,y,U,my_estate,POS1=pos2 )
call TRACK_probe_x(my_ering,MY_FIX,my_estate,node1=pos2 )
u=.not.check_stable



!n%auto=.false.
N=Y
write(6,*) "fractional part =",n%tune(1),n%tune(2)
write(6,*) "damping  =",n%damping(1),n%damping(2)

 
CALL KILL(Y);CALL ALLOC(Y);



ai=zero
ai=N%A_T



CALL KILL(ID);CALL KILL(Y);CALL KILL(N);
write(6,*) " done "

end subroutine compute_lattice_a_for_node

subroutine compute_lattice_function_monitors 
implicit none
TYPE(FIBRE), POINTER :: P
integer i

do i=1,size(monitors)

if(monitors(i)%full) then
p=>monitors(i)%p
 call compute_lattice_monitors(monitors(i)%at,p)
endif

enddo





end subroutine compute_lattice_function_monitors

subroutine compute_lattice_monitors(a,p)
implicit none
real(dp) a(:,:)
TYPE(FIBRE), POINTER :: P
INTEGER, ALLOCATABLE :: J(:),j1(:)
integer i,jj,k,n,ndel
lat_name="Some Lattice"

n=3  ! 1 2 3 are tunes
lat(0,0,1)=1
lat(0,0,2)=2
lat(0,0,3)=3
lat_name(lat(0,0,1))=" Phase x"
lat_name(lat(0,0,2))=" Phase y"
lat_name(lat(0,0,3))=" Phase t"
ndel=0
do i=1,4
do jj=i,4
do k=1,2
n=n+1
   p%i%beta(n)=a(i,2*k-1)*a(jj,2*k-1)
  p%i%beta(n)=a(i,2*k)*a(jj,2*k)+p%i%beta(n)
lat(i,jj,k)=n
lat(jj,i,k)=n
enddo
enddo
enddo


p%i%beta(lat(1,1,1))=sqrt(p%i%beta(lat(1,1,1)))
p%i%beta(lat(3,3,2))=sqrt(p%i%beta(lat(3,3,2)))
lat_name(lat(1,1,1))="Root betax = d<x x>/dI_1"
lat_name(lat(2,2,1))="gammax = d<px px>/dI_1"
lat_name(lat(1,2,1))="-alphax = d<x px>/dI_1"
lat_name(lat(3,3,2))="Root betay = d<y y>/dI_2"
lat_name(lat(4,4,2))="gammay = d<py py>/dI_2"
lat_name(lat(3,4,2))="-alphay = d<y py>/dI_2"

lat_name(lat(1,1,2))="betax_y = d<x x>/dI_2"
lat_name(lat(3,3,1))="betay_x = d<y y>/dI_1"
lat_name(lat(1,2,2))="-alphax_y = d<x px>/dI_2"
lat_name(lat(3,4,1))="-alphay_x = d<y py>/dI_1"
lat_name(lat(2,2,2))="gammax_y = d<px px>/dI_2"
lat_name(lat(4,4,1))="gammay_x = d<py py>/dI_1"

lat_name(lat(1,3,1))="     d<x y>/dI_1"
lat_name(lat(1,3,2))="     d<x y>/dI_2"
lat_name(lat(1,4,1))="     d<x py>/dI_1"
lat_name(lat(1,4,2))="     d<x py>/dI_2"
lat_name(lat(2,4,1))="     d<px py>/dI_1"
lat_name(lat(2,4,2))="     d<py py>/dI_2"
lat_name(lat(2,3,1))="     d<px y>/dI_1"
lat_name(lat(2,3,2))="     d<px y>/dI_2"





end subroutine compute_lattice_monitors


subroutine compute_lattice_function(ai,trac)
implicit none
INTEGER ND2,NPARA
TYPE(DAMAP) ID
TYPE(REAL_8) Y(6)
TYPE(NORMALFORM) N
INTEGER I, order,k
TYPE(FIBRE), POINTER :: P
INTEGER, ALLOCATABLE :: J(:)
REAL(DP) BETAX,fix0(6)
REAL(DP) PHASE(3),trace(2),testold(3),test,epsil,dph,ait(6,6)
real(dp), optional, intent(inout) :: ai(6,6)
logical positive,fix_p,tra
logical,optional :: trac
type(internal_state) state

if(present(ai)) ait=ai
tra=.true.
if(present(trac)) tra=trac

PHASE(:)=0.0_dp
TESTOLD(:)=0.0_dp
EPSIL=1.D-12
order=1
positive=.true.
fix_p=.true.
state=my_estate
!write(6,*) my_fix
!call print(my_estate,6)

my_fix(5)=my_delta
if(tra) then 
 CALL FIND_ORBIT_x(my_ering,MY_FIX,state,eps_gino,fibre1=MY_START)
else
 MY_FIX=0.d0
endif
    WRITE(6,'(6(1x,g21.14),a20)') my_fix," <-- closed orbit "

!write(6,*) my_fix
!call print(state,6)
!pause 123

CALL INIT(state,order,MY_NP)  !,BERZ,ND2,NPARA)

CALL ALLOC(ID);CALL ALLOC(Y);CALL ALLOC(N);
id=1
!Y=NPARA

if(tra) then

 Y=MY_FIX+ID

 CALL TRACK_probe_x(my_ering,Y,state,fibre1=MY_START)
 trace(1)=(y(1).sub.'1')+(y(2).sub.'01')
 trace(2)=(y(3).sub.'001')+(y(4).sub.'0001')
 write(6,*)  "Traces = ", trace(1),trace(2)
id=y
call checksymp(id)
 !n%auto=.false.
 N=Y
write(6,*) "fractional part =",n%tune(1:c_%nd) 
write(6,*) "damping  =",n%damping(1:c_%nd)
 id=N%A_T
else
 id=ait
endif
!CALL TRACK(my_ering,Y,MY_START,my_estate)


                         P=>my_ering%START
                         DO I=1,MY_START-1
                          P=>P%NEXT
                         ENDDO
CALL KILL(Y);CALL ALLOC(Y);
   p%i%beta(1:3)=phase(1:3)

!Y=NPARA
Y=MY_FIX+id

if(present(ai).and.tra)  then
 ai=zero
 ai=N%A_T
endif

call compute_lattice(y,p)

write(6,*) "MY_START, my_ering%N ",MY_START, my_ering%N
DO I=MY_START,MY_START+my_ering%N-1
!CALL TRACK(my_ering,Y,I,I+1,my_estate)
CALL TRACK_probe_x(my_ering,Y,state,fibre1=I,fibre2=I+1)

P=>P%NEXT

if(p%previous%mag%l<zero) then 
 positive=.not.positive 
endif

!if(positive) then
  TEST=ATAN2((Y(1).SUB.'01'),(Y(1).SUB.'10'))/TWOPI
  IF(TEST<0.D0.AND.abs(TEST)>EPSIL)TEST=TEST+one
  DPH=TEST-TESTOLD(1)
  IF(DPH<0.D0.AND.abs(DPH)>EPSIL) DPH=DPH+one
  IF(DPH>0.5D0) DPH=DPH-one
  
  
  PHASE(1)=PHASE(1)+DPH
  TESTOLD(1)=TEST
 ! write(20,*) p%mag%name,dph,phase(1)
  TEST=ATAN2((Y(3).SUB.'0001'),(Y(3).SUB.'0010'))/TWOPI
  IF(TEST<0.D0.AND.abs(TEST)>EPSIL)TEST=TEST+one
  DPH=TEST-TESTOLD(2)
  IF(DPH<0.D0.AND.abs(DPH)>EPSIL) DPH=DPH+one
  IF(DPH>0.5D0) DPH=DPH-one
 

  PHASE(2)=PHASE(2)+DPH
  TESTOLD(2)=TEST

  if(c_%nd2==6.and.c_%ndpt==0) then
  TEST=ATAN2((Y(5).SUB.'000001'),(Y(5).SUB.'000010'))/TWOPI
  IF(TEST<0.D0.AND.abs(TEST)>EPSIL)TEST=TEST+one
  DPH=TEST-TESTOLD(3)
  IF(DPH<0.D0.AND.abs(DPH)>EPSIL) DPH=DPH+one
  IF(DPH>0.5D0) DPH=DPH-one
  PHASE(3)=PHASE(3)+DPH
  TESTOLD(3)=TEST
  endif
 !endif ! positive





   p%i%beta(1:3)=phase(1:3)
 call compute_lattice(y,p)

ENDDO

!if(present(ai).and.(.not.tra))  then
! id=y
! ai=id
!endif

if(.not.positive) write(6,*) '$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$'
if(.not.positive) write(6,*) '$ Perhaps problems with Negative Drifts $'
if(.not.positive) write(6,*) '$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$'

write(6,*) "Phase Advances "
write(6,*) phase

CALL KILL(ID);CALL KILL(Y);CALL KILL(N);
write(6,*) " done "

end subroutine compute_lattice_function

subroutine print_res_lat(file)
implicit none
INTEGER ND2,NPARA
TYPE(DAMAP) ID
TYPE(REAL_8) Y(6)
TYPE(NORMALFORM) N
INTEGER I, order,k,mf
TYPE(FIBRE), POINTER :: P

REAL(sP) phase,sbxby2

character*(*) file

mf=20
open(unit=mf,file=file)



p=>my_ering%start

do i=1,my_ering%n
 phase=mres(1)*p%i%beta(1)+mres(2)*p%i%beta(2)+mres(3)*p%i%beta(3)
sbxby2=sqrt(p%i%beta(lat(1,1,1)))*sqrt(p%i%beta(lat(3,3,2)))**2
 write(mf,*) i,p%mag%name,phase,real(sbxby2,kind=sp)
 p=>p%next
enddo

CLOSE(MF)

end subroutine print_res_lat

subroutine print_res_(file)
implicit none
TYPE(DAMAP) ID,SC,A
TYPE(REAL_8) Y(6)
TYPE(NORMALFORM) N
TYPE(onelieexponent) onelie
type(pbresonance) hr
type(vecresonance) vr
INTEGER I, order,k,mf
TYPE(FIBRE), POINTER :: P
real(dp) ray(lnv),R(2),PREC
REAL(SP) RES(-3:3,-3:3)

REAL(sP) phase,sbxby2

character*(*) file
PREC=1.D-12

call kanalnummer(mf)
open(unit=mf,file=file)

my_fix=0.d0
CALL FIND_ORBIT(my_ering,MY_FIX,MY_START,my_estate,eps_gino)

CALL INIT(my_estate,MY_order,MY_NP)

CALL ALLOC(ID,SC,A);CALL ALLOC(Y);CALL ALLOC(N);CALL ALLOC(onelie);
CALL ALLOC(hr);CALL ALLOC(vr);

id=1

Y=MY_FIX+ID
CALL TRACK(my_ering,Y,MY_START,+my_estate)

id=y
id=id.sub.1
n=id
a=n%a_t

id=y
id=a**(-1)*id*a
sc=id.sub.1

sc=id*sc**(-1)

onelie=sc
vr=onelie%vector
hr=onelie%pb

PREC=1.d-9

write(mf,*) " tunes "
write(mf,*) n%tune
write(mf,*) " Non-linear part: one Lie exponent "
call print(onelie,mf,PREC)
write(mf,*) " Vector style Resonance "
call print(vr,mf,PREC)
write(mf,*) " Poisson style Resonance "
call print(hr,mf,PREC)


102  FORMAT(3(1X,E15.8))
101  FORMAT(i4,6(1X,E15.8))   !,1x,a16)


WRITE(6,*) " DONE "
CALL kill(ID,SC,A);CALL kill(Y);CALL kill(N);CALL kill(onelie);
CALL kill(hr);CALL kill(vr);

CLOSE(MF)

end subroutine print_res_


subroutine compute_lattice(y,p)
implicit none
TYPE(REAL_8) Y(6)
TYPE(FIBRE), POINTER :: P
INTEGER, ALLOCATABLE :: J(:),j1(:)
integer i,jj,k,n,ndel
ALLOCATE(J(6),j1(6))
lat_name="Some Lattice"

n=3  ! 1 2 3 are tunes
lat(0,0,1)=1
lat(0,0,2)=2
lat(0,0,3)=3
lat_name(lat(0,0,1))=" Phase x"
lat_name(lat(0,0,2))=" Phase y"
lat_name(lat(0,0,3))=" Phase t"
ndel=0
if(c_%ndpt/=0.and.c_%nd2==6) then
 ndel=1
endif
do i=1,c_%nd2-2*ndel
do jj=i,c_%nd2-2*ndel
do k=1,c_%nd-ndel
n=n+1
J=0 ;
J(2*k-1)=1
   p%i%beta(n)=(Y(i)%t.sub.J)*(Y(jj)%t.sub.J)
J=0 ;J(2*k)=1
  p%i%beta(n)=(Y(i)%t.sub.J)*(Y(jj)%t.sub.J)+p%i%beta(n)
lat(i,jj,k)=n
lat(jj,i,k)=n
enddo
enddo
enddo

if(c_%npara-c_%nd2==1.or.c_%ndpt/=0) then

 do i=1,c_%nd2-2*ndel
  n=n+1

  J=0 ;
  J(5)=1
    p%i%beta(n)=(Y(i)%t.sub.J)
  J=0
  lat(0,i,1)=n
enddo
 lat_name(lat(0,1,1))=" dispersion x"
 lat_name(lat(0,2,1))=" dispersion px"
 lat_name(lat(0,3,1))=" dispersion y"
 lat_name(lat(0,4,1))=" dispersion py"
elseif(c_%ndpt==0.and.c_%nd2==6) then
 do i=1,4
  n=n+1

  J=0 ;
  J1=0 ;
  J(5)=1
  J1(6)=1
    p%i%beta(n)=(Y(i)%t.sub.J)*(Y(6)%t.sub.J1)-(Y(i)%t.sub.J1)*(Y(6)%t.sub.J)
  J=0
  J1=0
  lat(0,i,1)=n
enddo
 lat_name(lat(0,1,1))=" dispersion x"
 lat_name(lat(0,2,1))=" dispersion px"
 lat_name(lat(0,3,1))=" dispersion y"
 lat_name(lat(0,4,1))=" dispersion py"

endif
p%i%beta(lat(1,1,1))=sqrt(p%i%beta(lat(1,1,1)))
p%i%beta(lat(3,3,2))=sqrt(p%i%beta(lat(3,3,2)))
!if(c_%nd2-2*ndel==6) p%i%beta(lat(5,5,3))=sqrt(p%i%beta(lat(5,5,3)))
lat_name(lat(1,1,1))="Root betax = d<x x>/dI_1"
lat_name(lat(2,2,1))="gammax = d<px px>/dI_1"
lat_name(lat(1,2,1))="-alphax = d<x px>/dI_1"
lat_name(lat(3,3,2))="Root betay = d<y y>/dI_2"
lat_name(lat(4,4,2))="gammay = d<py py>/dI_2"
lat_name(lat(3,4,2))="-alphay = d<y py>/dI_2"

lat_name(lat(1,1,2))="betax_y = d<x x>/dI_2"
lat_name(lat(3,3,1))="betay_x = d<y y>/dI_1"
lat_name(lat(1,2,2))="-alphax_y = d<x px>/dI_2"
lat_name(lat(3,4,1))="-alphay_x = d<y py>/dI_1"
lat_name(lat(2,2,2))="gammax_y = d<px px>/dI_2"
lat_name(lat(4,4,1))="gammay_x = d<py py>/dI_1"

lat_name(lat(1,3,1))="     d<x y>/dI_1"
lat_name(lat(1,3,2))="     d<x y>/dI_2"
lat_name(lat(1,4,1))="     d<x py>/dI_1"
lat_name(lat(1,4,2))="     d<x py>/dI_2"
lat_name(lat(2,4,1))="     d<px py>/dI_1"
lat_name(lat(2,4,2))="     d<py py>/dI_2"
lat_name(lat(2,3,1))="     d<px y>/dI_1"
lat_name(lat(2,3,2))="     d<px y>/dI_2"



DEALLOCATE(J,J1)


end subroutine compute_lattice

subroutine compute_nonlinear_tune(q,dtune)
implicit none
INTEGER ND2,NPARA
TYPE(DAMAP) ID
TYPE(REAL_8) Y(6)
TYPE(NORMALFORM) N
INTEGER I, order
TYPE(FIBRE), POINTER :: P
INTEGER, ALLOCATABLE :: J(:)
REAL(DP) q(3,0:6),dtune(3)
type(internal_state) state
logical fix_p

state=my_estate

if(state%nocavity) then

order=3
if(my_order>3) then
order=my_order
endif
if(state%only_4d) state=state+delta0

allocate(j(5))

!write(6,*) " Find Fixed Point "
!read(5,*) fix_p
!if(fix_p)then
my_fix(5)=my_delta
 CALL FIND_ORBIT(my_ering,MY_FIX,MY_START,my_estate,eps_gino)
!else
! MY_FIX=0.d0
!endif
write(6,*) my_fix


CALL INIT(state,order,MY_NP,BERZ,ND2,NPARA)

CALL ALLOC(ID);CALL ALLOC(Y);CALL ALLOC(N);
id=1
Y=NPARA

Y=MY_FIX+ID
CALL TRACK(my_ering,Y,MY_START,state)

!n%auto=.false.
N=Y

j=0
do i=0,order-1
j(5)=i

q(1,i)=(n%dhdj%v(1)).sub.j
q(2,i)=(n%dhdj%v(2)).sub.j

enddo

if(c_%nd2==4) then
dtune(1)=(n%dhdj%v(3)).sub.'1100'
dtune(2)=(n%dhdj%v(4)).sub.'0011'
dtune(3)=(n%dhdj%v(3)).sub.'0011'
elseif(c_%ndpt/=0) then
dtune(1)=(n%dhdj%v(4)).sub.'1100'
dtune(2)=(n%dhdj%v(5)).sub.'0011'
dtune(3)=(n%dhdj%v(4)).sub.'0011'
else
endif
 WRITE(6,'(a27,3(1x,g21.14))') "Tunes and alpha           =",n%tune(1),n%tune(2),n%tune(3)
 WRITE(6,'(a27,2(1x,g21.14))') "Chromaticities            =",q(1,1),q(2,1)
 WRITE(6,'(a27,2(1x,g21.14))') "2nd order chromaticities  =",q(1,2),q(2,2)
 WRITE(6,'(a27,3(1x,g21.14))') "Amplitude Tune Shift      =",dtune(1:3)

 
 
CALL KILL(Y);CALL ALLOC(Y);
CALL KILL(ID);CALL KILL(Y);CALL KILL(N)

deallocate(j)

write(6,*) " done "
else
write(6,*) "Only Coasting beams "
endif

end subroutine compute_nonlinear_tune

subroutine compute_one_turn(near,far,kob,kob0,fix3,turn)
implicit none
INTEGER ND2,NPARA
TYPE(DAMAP) ID
TYPE(REAL_8) Y(6)
TYPE(NORMALFORM) N,divergent_normal
INTEGER I,inear,ifar, no,mf,turn
TYPE(FIBRE), POINTER :: P
INTEGER, ALLOCATABLE :: J(:)
REAL(DP) coe,a,b,c,d,d_ey,ray(lnv),del,lam,k_r(4),r(4),rt,theta, theta0,fix3(6)
real(sp) near(2,3),far(2,3),kob(2,3),kob0(2,3),emi(2,3),normal_near(2,3)
type(pbresonance) f
type(pbfield) hrot
type(damap) N_res
type(onelieexponent) kobayashi
integer, allocatable :: llb(:)
!NRES,M(NDIM,NRESO)

CALL FIND_ORBIT(my_ering,MY_FIX,MY_START,my_estate,eps_gino)

no=my_order
if(no<3) no=3

CALL INIT(my_estate,no,MY_NP,BERZ,ND2,NPARA)
ALLOCATE(J(4))

CALL ALLOC(ID,N_res);CALL ALLOC(Y);CALL ALLOC(N);call alloc(f);call alloc(hrot);
call alloc(kobayashi)
id=1

Y=MY_FIX+ID
CALL TRACK(my_ering,Y,MY_START,my_estate)
id=y
open(20)
call print(id,20)

call KILL(kobayashi)
CALL KILL(ID,N_res);CALL KILL(Y);CALL KILL(N);call KILL(f);call KILL(hrot);

CALL INIT(my_estate,no+1,MY_NP,BERZ,ND2,NPARA)
CALL ALLOC(ID,N_res);CALL ALLOC(Y);CALL ALLOC(N);call alloc(f);call alloc(hrot);
call alloc(kobayashi)
rewind 20
call read(id,20)
close(20)

n%nres=5
n%M(1,1)=3
n%M(1,2)=6
n%M(1,3)=9
n%M(1,4)=12
n%M(1,5)=15

!n%auto=.false.
N=id          !  id is M of (5.22) n%normal is N of 5.22     B is in N%A_t
write(6,*) "Tunes ",n%tune(1),n%tune(2)
if(turn==1) then
mf=newfile
open(unit=mf,file="a_inv.txt")
id=n%a_t**(-1)
call print(id,mf)
mf=closefile
endif

f=n%normal%pb  ! n%normal IS REALLY FACTORED AS IN 5.11

!call print(n%normal%pb,6)
!pause
!call print(f%cos,6)
!pause
!call print(f%sin,6)
!pause

id=n%normal     !id of 5.22

hrot=(twopi/three/two)*((one.mono.'2')+(one.mono.'02'))   !  (5.42)
n_res=exp(hrot,id)                                           !  (5.42)

!N=n_res
allocate(llb(size(lielib_print)))
llb=lielib_print
lielib_print=1
!call idprset(1) ! print temporary results
kobayashi=n_res                  !5.43  5.44  (But other technique)


f=kobayashi%pb

mf=newfile
open(unit=mf,file="kobayashi_h.txt")
call print(f,mf)
mf=closefile

a=f%cos%h.sub.'11'
b=2*(f%cos%h.sub.'3')
c=2*(f%sin%h.sub.'3')
d=(f%cos%h.sub.'22')
d_ey=(f%cos%h.sub.'1111')

!call print(f%cos,6)
!pause
!call print(f%sin,6)
!pause

write(6,*) a,b,c,d,d_ey

lam=sqrt(c**2+b**2)
del=one-32*a*d/9/lam**2
if(del<0.d0) then
write(6,*) "No 1/3 resonance fixed points "
else
del=3.d0*lam*sqrt(del)/8.d0/d
k_r=0
r=0.d0
rt=-3.d0*lam*(-1)**0/8.d0/d+del
if(rt>0.d0) then
  i=1
  k_r(1)=0
  r(1)=rt
  else
  r(1)=-rt
  k_r(1)=1
endif

rt=-3.d0*lam*(-1)**1/8.d0/d+del
if(rt>0.d0) then
  i=1
  k_r(2)=1
  r(2)=rt
  else
  r(2)=-rt
  k_r(2)=0
endif

rt=-a*2.d0*(-1)**1/3.d0/lam
if(rt>0.d0) then
  i=1
  k_r(3)=1
  r(3)=rt
  else
  r(3)=-rt
  k_r(3)=0
endif
theta=atan2(c,b)
write(6,*) "theta = ",theta


f=n%normal%pb
b=2*(f%cos%h.sub.'3')
c=2*(f%sin%h.sub.'3')
d=(f%cos%h.sub.'22')
d_ey=(f%cos%h.sub.'1111')

lam=sqrt(c**2+b**2)

write(6,*) a,b,c,d,d_ey

rt=-a*2.d0*(-1)**1/3.d0/lam
if(rt>0.d0) then
  i=1
  k_r(4)=1
  r(4)=rt
  else
  r(4)=-rt
  k_r(4)=0
endif


theta0=atan2(c,b)
write(6,*) "theta0 = ",theta0


write(6,*) K_r
write(6,*) r

inear=1
ifar=2
if(r(1)>r(2)) then ; inear=2;ifar=1; endif;

do i=1,3

near(1,i)=r(inear)*cos((theta+k_r(inear)*pi)/3.d0+i*twopi/3.d0)
near(2,i)=-r(inear)*sin((theta+k_r(inear)*pi)/3.d0+i*twopi/3.d0)
far(1,i)=r(ifar)*cos((theta+k_r(ifar)*pi)/3.d0+i*twopi/3.d0)
far(2,i)=-r(ifar)*sin((theta+k_r(ifar)*pi)/3.d0+i*twopi/3.d0)

kob(1,i)=r(3)*cos((theta+k_r(3)*pi)/3.d0+i*twopi/3.d0)
kob(2,i)=-r(3)*sin((theta+k_r(3)*pi)/3.d0+i*twopi/3.d0)
kob0(1,i)=r(4)*cos((theta0+k_r(4)*pi)/3.d0+i*twopi/3.d0)
kob0(2,i)=-r(4)*sin((theta0+k_r(4)*pi)/3.d0+i*twopi/3.d0)

enddo

normal_near=near

write(6,*) " Near Fixed Points"
do i=1,3
write(6,*) near(1,i),near(2,i)
enddo
write(6,*) " Near Fixed Points (Kobayashi)"
do i=1,3
write(6,*) kob(1,i),kob(2,i)
enddo

write(6,*) " Far Fixed Points"
do i=1,3
write(6,*) far(1,i),far(2,i)
enddo

do i=1,3
ray=0.d0
ray(1:2)=near(1:2,i)
ray=n%A_t*ray
ray(1:2)=ray(1:2)+my_fix(1:2)
write(6,*) ray(1:2)
near(1:2,i)=ray(1:2)
enddo

do i=1,3
ray=0.d0
ray(1:2)=kob(1:2,i)
ray=(n%A_t.cut.3)*ray
ray(1:2)=ray(1:2)+my_fix(1:2)
write(6,*) ray(1:2)
kob(1:2,i)=ray(1:2)
enddo

do i=1,3
ray=0.d0
ray(1:2)=kob0(1:2,i)
ray=(n%A_t.sub.1)*ray
ray(1:2)=ray(1:2)+my_fix(1:2)
write(6,*) ray(1:2)
kob0(1:2,i)=ray(1:2)
enddo

do i=1,3
ray=0.d0
ray(1:2)=far(1:2,i)
ray=n%A_t*ray
ray(1:2)=ray(1:2)+my_fix(1:2)
far(1:2,i)=ray(1:2)
enddo


call alloc(divergent_normal)

divergent_normal=n_res

do i=1,3
ray=0.d0
ray(1:2)=normal_near(1:2,i)
write(6,*) ray(1:2)
ray=(divergent_normal%A_t)**(-1)*ray
emi(1:2,i)=ray(1:2)
write(6,*) i,emi(1,i)**2+emi(2,i)**2
enddo


call kill(divergent_normal)

endif


call KILL(kobayashi)
CALL KILL(ID,N_res);;CALL KILL(N);call KILL(f);call KILL(hrot);


DEALLOCATE(J)
if(turn==3) then
fix3=0.d0
fix3(5)=my_delta
fix3(1:2)=kob(1:2,2)
    CALL FIND_ORBIT(my_ering,fix3,1,my_estate,eps_gino,turns=3)

CALL INIT(my_estate,no,MY_NP,BERZ,ND2,NPARA)

CALL ALLOC(ID);CALL ALLOC(Y);CALL ALLOC(N);
id=1
Y=NPARA

Y=fix3+ID
write(6,*) fix3
CALL TRACK(my_ering,Y,MY_START,MY_START+3*my_ering%n,my_estate)
n%auto=.false.
n=y
my_fix=fix3
mf=newfile
open(unit=mf,file="a_inv.txt")
id=n%a_t**(-1)
call print(id,mf)
mf=closefile

CALL kill(ID);CALL kill(Y);CALL kill(N);
endif
write(6,*) " done "
lielib_print=llb
deallocate(llb)

end subroutine compute_one_turn



subroutine compute_map(file_map,file_a,file_ainv)
implicit none
INTEGER ND2,NPARA
TYPE(DAMAP) ID,RC,NC
TYPE(REAL_8) Y(6)
TYPE(NORMALFORM) N
TYPE(pbfield) h
TYPE(taylor) UNO
integer f_map,f_a,f_ai
character(*) file_map,file_a,file_ainv
integer p,m,i,k

CALL FIND_ORBIT(my_ering,MY_FIX,MY_START,my_estate,eps_gino)

m=0
do i=1,3
m=m+iabs(mres(i))
enddo

if(m/=0) then
 write(6,'(a23,3(1x,i4))') " Resonance left in map ",mres
endif

GLOBAL_VERBOSE=.TRUE.

CALL INIT(my_estate,my_order,MY_NP,BERZ,ND2,NPARA)

CALL ALLOC(ID,RC,NC);CALL ALLOC(Y);CALL ALLOC(N);CALL ALLOC(H);
CALL ALLOC(UNO)
!     integer NRES,M(NDIM,NRESO),PLANE(NDIM)

k=0
if(m/=0) then
 do i=1,my_order
 if(mod(i,m)==0) then
  k=k+1
  n%m(1,k)=k*mres(1)
  n%m(2,k)=k*mres(2)
  n%m(3,k)=k*mres(3)
 endif 
 enddo
endif
n%nres=k
 if(k/=0) write(6,'(1(1x,i4),a29)')  k," Resonance terms left in map "

id=1
Y=NPARA

Y=MY_FIX+ID
CALL TRACK(my_ering,Y,MY_START,my_estate)

!n%auto=.false.
N=Y
write(6,*) "fractional part =",n%tune(1),n%tune(2)
id=y

f_map=newfile
f_a=newfile
f_ai=newfile
open(unit=f_map,file=file_map)
open(unit=f_a,file=file_a)
open(unit=f_ai,file=file_ainv)

call daprint(id,f_map)
call daprint(n%a_t,f_a)
id=n%a_t**(-1)
call daprint(id,f_ai)

IF(M/=0) THEN
   H%H=ZERO
   DO I=1,C_%ND
   H%H=H%H+MRES(I)*((one.MONO.(2*I-1))**2+(one.MONO.(2*I))**2)/TWO
   ENDDO

   H%H=TWOPI*mresp/(MRES(1)**2+MRES(2)**2+MRES(3)**2)*H%H
   RC=1
   RC=TEXP(H,RC)
   NC=N%NORMAL

   NC=NC*RC
    N=Nc
   write(6,*) " Tunes of co-moving map =",n%tune(1),n%tune(2)

   UNO=N%normal%pb
   uno=uno*n%a_t**(-1)

   id=1
   id%v(1)=uno
   id%v(2)=mres(1)*(id%v(3)**2+id%v(4)**2)-mres(2)*(id%v(1)**2+id%v(2)**2)
   call daprint(id,f_ai)
else

   id=1
   id%v(1)=(id%v(1)**2+id%v(2)**2)
   id%v(2)=(id%v(3)**2+id%v(4)**2) 
   call daprint(id,f_ai)

ENDIF

call print(N%dhdj,f_map)

f_map=closefile
f_a=closefile
f_ai=closefile
!write(6,*) f_map,f_a,f_ai
CALL KILL(ID,RC,NC);CALL KILL(Y);CALL KILL(N);CALL KILL(H);
CALL KILL(UNO)
write(6,*) " done "

GLOBAL_VERBOSE=.FALSE.

end subroutine compute_map


! New Style fitting routines using IMSL


end module build_lattice
